package ch.reevolt.android.simulation;

import java.util.LinkedList;

import android.graphics.Rect;

import ch.reevolt.android.generics.Vector2D;

/**
 * A simple physics simulator for the inf1 project.
 * 
 * @version 1.2, March 2012
 * @author <a href='mailto:pandre.mudry&#64;hevs.ch'> Pierre-Andre Mudry</a>
 */
public class PhysicsSimulator {

	/**
	 * This represents the borders of the simulated area (for collisions)
	 */
	double width;
	double height;

	private final boolean VERBOSE_PHYSICS = false;

	/**
	 * The objects that require physics simulation (objects that move)
	 */
	private LinkedList<Simulatable> sim_objects = new LinkedList<Simulatable>();

	public PhysicsSimulator(int width, int height) {
		this.width = width;
		this.height = height;
	}

	/**
	 * Adds a new object to the simulation framework
	 * 
	 * @param o
	 *            The object to be added
	 */
	public void addSimulatableObject(Simulatable o) {
		sim_objects.add(o);

		if (VERBOSE_PHYSICS)
			System.out.println("[Physics] Object added, #objects simulated "
					+ sim_objects.size());
	}
	
	public void removeSimulatableObject(Simulatable object){
		sim_objects.remove(object);
	}

	/**
	 * Simulates all the objects that ought to be simulated
	 * 
	 * @return
	 */
	public void simulate_step() {
		if (sim_objects.size() == 0)
			return;

		for (int i = 0; i < sim_objects.size(); i++) {
			Simulatable s = sim_objects.get(i);

			s.step();

			if (s instanceof PhysicalObject ) {
				PhysicalObject p = (PhysicalObject) s;

				/** Physics calculations */
				/** Define vector of friction and gravity */
				Vector2D frottement = new Vector2D(-Constants.AIR_FRICTION
						* p.getSpeed().x, -Constants.AIR_FRICTION
						* p.getSpeed().y);
				Vector2D gravity = new Vector2D(
						Constants.X_ACCEL * p.getMass(), Constants.Y_ACCEL
								* p.getMass());

				/** Calculate sum of force */
				Vector2D sumForce = new Vector2D(frottement.x + gravity.x,
						frottement.y + gravity.y);

				/**
				 * Calculate speed vector, acceleration vector and postion
				 * vector
				 */
				Vector2D acceleration = new Vector2D(sumForce.x / p.getMass(),
						sumForce.y / p.getMass());
				Vector2D speed = new Vector2D(p.getSpeed().x + acceleration.x
						* Constants.DELTA_TIME, p.getSpeed().y + acceleration.y
						* Constants.DELTA_TIME);
				Vector2D position = new Vector2D(p.getPosition().x + speed.x
						* Constants.DELTA_TIME, p.getPosition().y + speed.y
						* Constants.DELTA_TIME);

				/** Add calculate vector to the physical object */
				p.setAcceleration(acceleration);
				p.setSpeed(speed);
				p.setPosition(position);
				
				if(p instanceof DynamicObject){
					/** General particle movement **/
					groundcollision((DynamicObject)p);

					/** Handle collisions with other objects **/
					handleCollisions((DynamicObject)p);
				}				
			}
		}
	}

	public void groundcollision(DynamicObject p) {

		// int ecin = (int) (p.s.norm() / 2 * p.mass);
		if (p.getBoundingBox().left < 0 || p.getBoundingBox().right > width) {
			p.getPosition().x -= Constants.DELTA_TIME * p.getSpeed().x;
			p.getPosition().y -= Constants.DELTA_TIME * p.getSpeed().y;
			p.setSpeed(new Vector2D(-p.s.x * Constants.DAMPING_FACTOR, p.s.y
					* Constants.DAMPING_FACTOR));

			// p.notifyCollision(ecin, p);
		} else if (p.getBoundingBox().top < 0
				|| p.getBoundingBox().bottom > height) {
			p.getPosition().x -= Constants.DELTA_TIME * p.getSpeed().x;
			p.getPosition().y -= Constants.DELTA_TIME * p.getSpeed().y;
			p.setSpeed(new Vector2D(p.s.x * Constants.DAMPING_FACTOR, -p.s.y
					* Constants.DAMPING_FACTOR));
		}
	}

	private void handleCollisions(DynamicObject p) {

		// For each object, find potential obstacles among other objects
		for (int j = 0; j < sim_objects.size(); j++) {
			Simulatable aSimObj = sim_objects.get(j);
			DynamicObject q = aSimObj instanceof DynamicObject ? (DynamicObject) aSimObj
					: null;

			if (p != null && q != null && p != q && collides(p, q)) {
				// Remove interpenetration
				p.getPosition().x -= Constants.DELTA_TIME * p.getSpeed().x;
				p.getPosition().y -= Constants.DELTA_TIME * p.getSpeed().y;

				// Formula of elastic collision
				float m1 = p.getMass();
				float m2 = q.getMass();
				float v1x = p.getSpeed().x;
				float v1y = p.getSpeed().y;
				float v2x = q.getSpeed().x;
				float v2y = q.getSpeed().y;

				p.getSpeed().x = Constants.DAMPING_FACTOR
						* (v1x * (m1 - m2) / (m1 + m2))
						+ (2 * m2 * v2x / (m1 + m2));
				p.getSpeed().y = Constants.DAMPING_FACTOR
						* (v1y * (m1 - m2) / (m1 + m2))
						+ (2 * m2 * v2y / (m1 + m2));

				q.getSpeed().x = Constants.DAMPING_FACTOR
						* (2 * m1 * v1x / (m1 + m2))
						+ (v2x * (m2 - m1) / (m1 + m2));
				q.getSpeed().y = Constants.DAMPING_FACTOR
						* (2 * m1 * v1y / (m1 + m2))
						+ (v2y * (m2 - m1) / (m1 + m2));

				// Impact total kinetic energy
				int energy;
				energy = (int) (0.5 * q.getMass() * q.getSpeed().norm() * q
						.getSpeed().norm());
				energy += (int) (0.5 * p.getMass() * p.getSpeed().norm() * p
						.getSpeed().norm());

				// Notify the objects of the impact and destroy them if needed
				if (p.notifyCollision(energy, q)) {
					removeObjectFromSim(p);
				}

				if (q.notifyCollision(energy, p)) {
					removeObjectFromSim(q);
				}
			}
		}
	}

	private void removeObjectFromSim(DynamicObject d) {
		d.removedFromSim();
		sim_objects.remove(d);
	}

	private boolean collides(DynamicObject a, DynamicObject b) {
		// Object should NOT collide with themselves
		if (!b.equals(a)) {
			Rect bb1 = a.getBoundingBox();
			Rect bb2 = b.getBoundingBox();

			try {
				if (bb1 == null)
					throw new NullPointerException(
							"Bouding box not defined for " + a.getClass());
				if (bb2 == null)
					throw new NullPointerException(
							"Bouding box not defined for " + b.getClass());
			} catch (Exception e) {
				e.printStackTrace();
				System.exit(-1);
			}

			return bb1.intersect(bb2);
		} else
			return false;
	}

	public void removeAllObjectsfromSim() {
		sim_objects.clear();
	}
}
